// MIT License

// Copyright (c) 2019 Erin Catto

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "test.h"
#include "settings.h"
#include <stdio.h>

void DestructionListener::SayGoodbye(b2Joint* joint)
{
  if (test->m_mouseJoint == joint)
  {
    test->m_mouseJoint = NULL;
  }
  else
  {
    test->JointDestroyed(joint);
  }
}

Test::Test()
{
  b2Vec2 gravity;
  gravity.Set(0.0f, -10.0f);
  m_world = new b2World(gravity);
  
  const b2ParticleSystemDef particleSystemDef;
  m_particleSystem = m_world->CreateParticleSystem(&particleSystemDef);
  
  m_particleSystem->SetGravityScale(0.4f);
  m_particleSystem->SetDensity(1.2f);

  m_bomb = NULL;
  m_textLine = 30;
  m_textIncrement = 13;
  m_mouseJoint = NULL;
  m_pointCount = 0;

  m_destructionListener.test = this;
  m_world->SetDestructionListener(&m_destructionListener);
  m_world->SetContactListener(this);
  m_world->SetDebugDraw(&g_debugDraw);
  
  m_mouseTracing = false;
  m_bombSpawning = false;

  m_stepCount = 0;

  b2BodyDef bodyDef;
  m_groundBody = m_world->CreateBody(&bodyDef);

  memset(&m_maxProfile, 0, sizeof(b2Profile));
  memset(&m_totalProfile, 0, sizeof(b2Profile));
}

Test::~Test()
{
  // By deleting the world, we delete the bomb, mouse joint, etc.
  delete m_world;
  m_world = NULL;
}

void Test::PreSolve(b2Contact* contact, const b2Manifold* oldManifold)
{
  const b2Manifold* manifold = contact->GetManifold();

  if (manifold->pointCount == 0)
  {
    return;
  }

  b2Fixture* fixtureA = contact->GetFixtureA();
  b2Fixture* fixtureB = contact->GetFixtureB();

  b2PointState state1[b2_maxManifoldPoints], state2[b2_maxManifoldPoints];
  b2GetPointStates(state1, state2, oldManifold, manifold);

  b2WorldManifold worldManifold;
  contact->GetWorldManifold(&worldManifold);

  for (int32 i = 0; i < manifold->pointCount && m_pointCount < k_maxContactPoints; ++i)
  {
    ContactPoint* cp = m_points + m_pointCount;
    cp->fixtureA = fixtureA;
    cp->fixtureB = fixtureB;
    cp->position = worldManifold.points[i];
    cp->normal = worldManifold.normal;
    cp->state = state2[i];
    cp->normalImpulse = manifold->points[i].normalImpulse;
    cp->tangentImpulse = manifold->points[i].tangentImpulse;
    cp->separation = worldManifold.separations[i];
    ++m_pointCount;
  }
}

void Test::DrawTitle(const char *string)
{
    g_debugDraw.DrawString(5, 5, string);
    m_textLine = int32(26.0f);
}

class QueryCallback : public b2QueryCallback
{
public:
  QueryCallback(const b2Vec2& point)
  {
    m_point = point;
    m_fixture = NULL;
  }

  bool ReportFixture(b2Fixture* fixture) override
  {
    b2Body* body = fixture->GetBody();
    if (body->GetType() == b2_dynamicBody)
    {
      bool inside = fixture->TestPoint(m_point);
      if (inside)
      {
        m_fixture = fixture;

        // We are done, terminate the query.
        return false;
      }
    }

    // Continue the query.
    return true;
  }

  b2Vec2 m_point;
  b2Fixture* m_fixture;
};

class QueryCallback2 : public b2QueryCallback
{
public:
  QueryCallback2(b2ParticleSystem* particleSystem,
           const b2Shape* shape, const b2Vec2& velocity)
  {
    m_particleSystem = particleSystem;
    m_shape = shape;
    m_velocity = velocity;
  }

  bool ReportFixture(b2Fixture* fixture)
  {
    B2_NOT_USED(fixture);
    return false;
  }

  bool ReportParticle(const b2ParticleSystem* particleSystem, int32 index)
  {
    if (particleSystem != m_particleSystem)
      return false;

    b2Transform xf;
    xf.SetIdentity();
    b2Vec2 p = m_particleSystem->GetPositionBuffer()[index];
    if (m_shape->TestPoint(xf, p))
    {
      b2Vec2& v = m_particleSystem->GetVelocityBuffer()[index];
      v = m_velocity;
    }
    return true;
  }

  b2ParticleSystem* m_particleSystem;
  const b2Shape* m_shape;
  b2Vec2 m_velocity;
};

void Test::MouseDown(const b2Vec2& p)
{
  m_mouseWorld = p;
  m_mouseTracing = true;
  m_mouseTracerPosition = p;
  m_mouseTracerVelocity = b2Vec2_zero;
  
  if (m_mouseJoint != NULL)
  {
    return;
  }

  // Make a small box.
  b2AABB aabb;
  b2Vec2 d;
  d.Set(0.001f, 0.001f);
  aabb.lowerBound = p - d;
  aabb.upperBound = p + d;

  // Query the world for overlapping shapes.
  QueryCallback callback(p);
  m_world->QueryAABB(&callback, aabb);

  if (callback.m_fixture)
  {
    float frequencyHz = 5.0f;
    float dampingRatio = 0.7f;

    b2Body* body = callback.m_fixture->GetBody();
    b2MouseJointDef jd;
    jd.bodyA = m_groundBody;
    jd.bodyB = body;
    jd.target = p;
    jd.maxForce = 1000.0f * body->GetMass();
    b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB);

    m_mouseJoint = (b2MouseJoint*)m_world->CreateJoint(&jd);
    body->SetAwake(true);
  }
}

void Test::SpawnBomb(const b2Vec2& worldPt)
{
  m_bombSpawnPoint = worldPt;
  m_bombSpawning = true;
}
    
void Test::CompleteBombSpawn(const b2Vec2& p)
{
  if (m_bombSpawning == false)
  {
    return;
  }

  const float multiplier = 30.0f;
  b2Vec2 vel = m_bombSpawnPoint - p;
  vel *= multiplier;
  LaunchBomb(m_bombSpawnPoint,vel);
  m_bombSpawning = false;
}

void Test::ShiftMouseDown(const b2Vec2& p)
{
  m_mouseWorld = p;
  
  if (m_mouseJoint != NULL)
  {
    return;
  }

  SpawnBomb(p);
}

void Test::MouseUp(const b2Vec2& p)
{
  m_mouseTracing = false;

  if (m_mouseJoint)
  {
    m_world->DestroyJoint(m_mouseJoint);
    m_mouseJoint = NULL;
  }
  
  if (m_bombSpawning)
  {
    CompleteBombSpawn(p);
  }
}

void Test::MouseMove(const b2Vec2& p)
{
  m_mouseWorld = p;
  
  if (m_mouseJoint)
  {
    m_mouseJoint->SetTarget(p);
  }
}

void Test::LaunchBomb()
{
  b2Vec2 p(RandomFloat(-15.0f, 15.0f), 30.0f);
  b2Vec2 v = -5.0f * p;
  LaunchBomb(p, v);
}

void Test::LaunchBomb(const b2Vec2& position, const b2Vec2& velocity)
{
  if (m_bomb)
  {
    m_world->DestroyBody(m_bomb);
    m_bomb = NULL;
  }

  b2BodyDef bd;
  bd.type = b2_dynamicBody;
  bd.position = position;
  bd.bullet = true;
  m_bomb = m_world->CreateBody(&bd);
  m_bomb->SetLinearVelocity(velocity);
  
  b2CircleShape circle;
  circle.m_radius = 0.3f;

  b2FixtureDef fd;
  fd.shape = &circle;
  fd.density = 20.0f;
  fd.restitution = 0.0f;
  
  b2Vec2 minV = position - b2Vec2(0.3f,0.3f);
  b2Vec2 maxV = position + b2Vec2(0.3f,0.3f);
  
  b2AABB aabb;
  aabb.lowerBound = minV;
  aabb.upperBound = maxV;

  m_bomb->CreateFixture(&fd);
}

void Test::Step(Settings& settings)
{
  float timeStep = settings.m_hertz > 0.0f ? 1.0f / settings.m_hertz : float(0.0f);

  if (settings.m_pause)
  {
    if (settings.m_singleStep)
    {
      settings.m_singleStep = 0;
    }
    else
    {
      timeStep = 0.0f;
    }

    g_debugDraw.DrawString(5, m_textLine, "****PAUSED****");
    m_textLine += m_textIncrement;
  }

  uint32 flags = 0;
  flags += settings.m_drawShapes * b2Draw::e_shapeBit;
  flags += settings.m_drawJoints * b2Draw::e_jointBit;
  flags += settings.m_drawParticles * b2Draw::e_particleBit;
  flags += settings.m_drawAABBs * b2Draw::e_aabbBit;
  flags += settings.m_drawCOMs * b2Draw::e_centerOfMassBit;
  g_debugDraw.SetFlags(flags);

  m_world->SetAllowSleeping(settings.m_enableSleep);
  m_world->SetWarmStarting(settings.m_enableWarmStarting);
  m_world->SetContinuousPhysics(settings.m_enableContinuous);
  m_world->SetSubStepping(settings.m_enableSubStepping);

  m_pointCount = 0;

  m_world->Step(timeStep, settings.m_velocityIterations, settings.m_positionIterations, settings.m_particleIterations);

  m_world->DebugDraw();
    g_debugDraw.Flush();

  if (timeStep > 0.0f)
  {
    ++m_stepCount;
  }

  if (settings.m_drawStats)
  {
    int32 bodyCount = m_world->GetBodyCount();
    int32 contactCount = m_world->GetContactCount();
    int32 jointCount = m_world->GetJointCount();
    g_debugDraw.DrawString(5, m_textLine, "bodies/contacts/joints = %d/%d/%d", bodyCount, contactCount, jointCount);
    m_textLine += m_textIncrement;

    int32 proxyCount = m_world->GetProxyCount();
    int32 height = m_world->GetTreeHeight();
    g_debugDraw.DrawString(5, m_textLine, "proxies/height = %d/%d/%g", proxyCount, height);
    m_textLine += m_textIncrement;
  }

  // Track maximum profile times
  {
    const b2Profile& p = m_world->GetProfile();
    m_maxProfile.step = b2Max(m_maxProfile.step, p.step);
    m_maxProfile.collide = b2Max(m_maxProfile.collide, p.collide);
    m_maxProfile.solve = b2Max(m_maxProfile.solve, p.solve);
    m_maxProfile.solveTOI = b2Max(m_maxProfile.solveTOI, p.solveTOI);
    m_maxProfile.broadphase = b2Max(m_maxProfile.broadphase, p.broadphase);

    m_totalProfile.step += p.step;
    m_totalProfile.collide += p.collide;
    m_totalProfile.solve += p.solve;
    m_totalProfile.solveTOI += p.solveTOI;
    m_totalProfile.broadphase += p.broadphase;
  }

  if (settings.m_drawProfile)
  {
    const b2Profile& p = m_world->GetProfile();

    b2Profile aveProfile;
    memset(&aveProfile, 0, sizeof(b2Profile));
    if (m_stepCount > 0)
    {
      float scale = 1.0f / m_stepCount;
      aveProfile.step = scale * m_totalProfile.step;
      aveProfile.collide = scale * m_totalProfile.collide;
      aveProfile.solve = scale * m_totalProfile.solve;
      aveProfile.solveTOI = scale * m_totalProfile.solveTOI;
      aveProfile.broadphase = scale * m_totalProfile.broadphase;
    }

    g_debugDraw.DrawString(5, m_textLine, "step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, m_maxProfile.step);
    m_textLine += m_textIncrement;
    g_debugDraw.DrawString(5, m_textLine, "collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, m_maxProfile.collide);
    m_textLine += m_textIncrement;
    g_debugDraw.DrawString(5, m_textLine, "solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, m_maxProfile.solve);
    m_textLine += m_textIncrement;
    g_debugDraw.DrawString(5, m_textLine, "solveTOI [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveTOI, aveProfile.solveTOI, m_maxProfile.solveTOI);
    m_textLine += m_textIncrement;
    g_debugDraw.DrawString(5, m_textLine, "broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase);
    m_textLine += m_textIncrement;
  }

  if (m_bombSpawning)
  {
    b2Color c;
    c.Set(0.0f, 0.0f, 1.0f);
    g_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c);

    c.Set(0.8f, 0.8f, 0.8f);
    g_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c);
  }

  if (m_mouseTracing && !m_mouseJoint)
  {
    float32 delay = 0.1f;
    b2Vec2 acceleration = 2 / delay * (1 / delay * (m_mouseWorld - m_mouseTracerPosition) - m_mouseTracerVelocity);
    m_mouseTracerVelocity += timeStep * acceleration;
    m_mouseTracerPosition += timeStep * m_mouseTracerVelocity;
    b2CircleShape shape;
    shape.m_p = m_mouseTracerPosition;
    shape.m_radius = 2;
    QueryCallback2 callback(m_particleSystem, &shape, m_mouseTracerVelocity);
    b2AABB aabb;
    b2Transform xf;
    xf.SetIdentity();
    shape.ComputeAABB(&aabb, xf);
    m_world->QueryAABB(&callback, aabb);
  }

  if (settings.m_drawContactPoints)
  {
    const float k_impulseScale = 0.1f;
    const float k_axisScale = 0.3f;

    for (int32 i = 0; i < m_pointCount; ++i)
    {
      ContactPoint* point = m_points + i;

      if (point->state == b2_addState)
      {
        // Add
        g_debugDraw.DrawPoint(point->position, 10.0f, b2Color(0.3f, 0.95f, 0.3f));
      }
      else if (point->state == b2_persistState)
      {
        // Persist
        g_debugDraw.DrawPoint(point->position, 5.0f, b2Color(0.3f, 0.3f, 0.95f));
      }

      if (settings.m_drawContactNormals == 1)
      {
        b2Vec2 p1 = point->position;
        b2Vec2 p2 = p1 + k_axisScale * point->normal;
        g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.9f));
      }
      else if (settings.m_drawContactImpulse == 1)
      {
        b2Vec2 p1 = point->position;
        b2Vec2 p2 = p1 + k_impulseScale * point->normalImpulse * point->normal;
        g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
      }

      if (settings.m_drawFrictionImpulse == 1)
      {
        b2Vec2 tangent = b2Cross(point->normal, 1.0f);
        b2Vec2 p1 = point->position;
        b2Vec2 p2 = p1 + k_impulseScale * point->tangentImpulse * tangent;
        g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
      }
    }
  }
}

void Test::ShiftOrigin(const b2Vec2& newOrigin)
{
  m_world->ShiftOrigin(newOrigin);
}

TestEntry g_testEntries[MAX_TESTS] = { {nullptr} };
int g_testCount = 0;

int RegisterTest(const char* category, const char* name, TestCreateFcn* fcn)
{
  int index = g_testCount;
  if (index < MAX_TESTS)
  {
    g_testEntries[index] = { category, name, fcn };
    ++g_testCount;
    return index;
  }

  return -1;
}
